#include "scene_obs_pass.h"


constexpr double safeDis = 15.0;

// 静态绕障
ObsPassStatic::ObsPassStatic() {
    // 道路
    road0 = std::make_unique<RoadNormal>(250.0);
    // 车辆
    car0 = std::make_unique<CarNormal>(SWIDTH / 2.0, SWIDTH / 2.0 + 500);
    car0->speed = -4.0;
    // 锥桶
    cone = std::make_unique<Cone>(SWIDTH / 2.0 - 100.0, SHEIGHT / 2.0 - 200.0);
    // car0->CoutInfo();
    ShowScene();
    system("pause");
}

void ObsPassStatic::ShowScene() {
    BeginBatchDraw();
    cleardevice();

    road0->ShowRoad();
    car0->ShowCar(BLACK);
    cone->ShowCone();
    // 绘制轨迹线
    if (SHOWCIRCLE && car0->p_center) {
        car0->ShowCircle();
    }

    EndBatchDraw();
    Delay(DELAYTIME);
}

bool ObsPassStatic::Planning_process() {
    // 开始绕障的位置
    double start_line = cone->p_center->y + cone->r + start_dis + car0->car_length;
    // 直行到绕障位置
    UniformStraight(car0->pmidr->y - start_line);
    std::cout << "------------------straight--------------------" << std::endl;
    // 计算左右间距
    double left_space = cone->p_center->x - cone->r - road0->left_boundary;
    double right_space = road0->right_boundary - cone->p_center->x - cone->r;
    // 计算左右目标点
    double target_l_x = road0->left_boundary + left_space / 2.0;
    double target_r_x = road0->right_boundary - right_space / 2.0;
    double target_l_y = cone->p_center->y;
    double target_r_y = cone->p_center->y;
    Point target_l(target_l_x, target_l_y);
    Point target_r(target_r_x, target_r_y);
    // 判断选择单移线还是双移线
    LaneChangeType type;
    if (cone->p_center->y - cone->r > start_dis + car0->car_length) {
        type = LaneChangeType::doubleType;
    } else {
        type = LaneChangeType::singleType;
    }
    std::cout << "type: " << type << ", ";
    // 如果左边宽度够，优先左绕
    if (left_space > 1.5 * car0->car_width) {
        std::cout << "left pass   ";
        LaneChange(target_l, type);
    } else {
        if (right_space > 1.5 * car0->car_width) {
            // 左边宽度不够，选择右绕
            std::cout << "right pass   ";
            LaneChange(target_r, type);
        } else {
            // 左右宽度都不够，执行停障
            std::cout << "no left/right pass, failed   ";
            double stop_line = cone->p_center->y + cone->r + safeDis;
            UniformAccByDis(car0->pmidr->y - stop_line, 0.0);
            return false;
        }
    }

    // 绕障完成后，继续直行
    car0->UpdatePmidr();
    UniformStraight(car0->pmidr->y - 0.0);
    return true;
}